/*
 * Project:    RoboBerDuino
 * Author:       Marco Tili
 *
 * Platform:    Arduino Due
 *
 * Filename:            TBD
 * Version:           00.02
 *
 * Description:
 *       Software for slave device. Wait for command from Master then
 *       execute command related actions and answer to such command.
 *       Actions include: motor control, proximity sensors reading,
 *       batteries sensing, lights control, etc...
 *
 * Changelog:
 *       2013-03-22   | M.Tili
 *                    | First Issue
 *       2013-03-26   | M.Tili
 *                    | Made 1 struct for tx and rx data
 *                    | Added function BuildEchoTx 
 *       2013-04-23   | M.Tili
 *                    | Fixed BuildEchoTx
 *
 */


#ifndef VM_WORK
  #include <c:/archivio/vm_shares/my_svn_repo/raspduinobot/trunk/src/rbd_arduino/conf.h>
  #include <c:/archivio/vm_shares/my_svn_repo/raspduinobot/trunk/src/rbd_arduino/protodefs.h>
  //#include <e:/svn-repo/raspduinobot/trunk/src/rbd_arduino/conf.h>
  //#include <e:/svn-repo/raspduinobot/trunk/src/rbd_arduino/protodefs.h>
#else
  #include <e:/my_svn_repo/raspduinobot/trunk/src/rbd_arduino/conf.h>
  #include <e:/my_svn_repo/raspduinobot/trunk/src/rbd_arduino/protodefs.h>
#endif

//#SECTION : TYPEs
typedef unsigned char  Bool;
typedef unsigned char  UByte;
typedef char           SByte;
typedef unsigned char  UInt8;
typedef char           SInt8;
typedef unsigned short UInt16;
typedef short          SInt16;
typedef unsigned int   UInt32;
typedef int            SInt32;
//#

//#SECTION : DEFINEs
#ifdef TRUE
  #undef TRUE
#endif
#define TRUE 0x01
#ifdef FALSE
  #undef FALSE
#endif
#define FALSE 0x00

#ifndef OUTPUT
  #define OUTPUT 1
#endif
#ifndef INPUT
  #define INPUT 0
#endif
//#

//#SECTION : ENUMs
typedef enum {
  E_FSMMAIN_CONN_WAIT,
  E_FSMMAIN_CONN_ESTAB,
  E_FSMMAIN_CONN_CLOSED
} e_fsm_main;

typedef enum {
  E_FSMRX_WSYNC1,
  E_FSMRX_WSYNC2,
  E_FSMRX_WSYNC3,
  E_FSMRX_HEADER,
  E_FSMRX_BODY,
  E_FSMRX_FOOTER,
  E_FSMRX_COMPLETE,
  E_FSMRX_BADCRC
} e_fsm_comm;
//#

//#SECTION : STRUCTURES
typedef struct tconf {
  Bool   isValid;
  UInt32 serialBaudrate;
  UInt16 serialMaxLen;
  UInt32 ledDelay;
  UInt8  ledPin;
} t_conf;

typedef struct ttxrx{
  UByte       buffer[FRAME_LENGTH];
  UInt16      length;
  UInt16      nrBytes;
  Bool        enRead;
  Bool        enWrite;
  e_fsm_comm  fsm;
} t_txrx;

typedef struct tcomm {
  t_txrx  rx;
  t_txrx  tx;
} t_comm;
//#

//#SECTION : VARIABLEs
t_comm     gComm;   //TBD use this var
e_fsm_main fsmMain;
t_conf     config;
UInt32     timeNow;
Bool       ledStatePrev = FALSE;
UInt32     ledStateChTime = 0;
//#

//#SECTION : FUNCTIONs PROTOTYPEs
static void LedToggleVitality();
static UInt16 DoRx(UByte *pBuff);
static void UpdateRx();
static UInt16 DoTx(UByte *pBuff,UInt16 *size);
static void UpdateTx();
static Bool BuildEchoTx(UByte *pBuff);
//#

void setup()
{
  UInt16 i = 0;
  
  timeNow = millis();
  config.serialBaudrate = D_SERIAL_BAUDRATE;
  config.serialMaxLen   = D_SERIAL_MAX_LENGTH;
  config.ledPin         = D_LED_BUILTIN_PIN;
  config.ledDelay       = D_LED_TIMEOUT;
  config.isValid        = TRUE;
  pinMode(config.ledPin, OUTPUT);
  SerialUSB.begin(config.serialBaudrate);
  fsmMain               = E_FSMMAIN_CONN_WAIT;
  
  //Init comm var
  gComm.rx.fsm     = E_FSMRX_WSYNC1;
  gComm.rx.length  = 0;
  gComm.rx.nrBytes = 0;
  gComm.rx.enRead  = FALSE;
  gComm.tx.enWrite = TRUE;
  gComm.tx.fsm     = E_FSMRX_WSYNC1;
  gComm.tx.length  = 0;
  gComm.tx.nrBytes = 0;
  gComm.tx.enRead  = FALSE;
  gComm.tx.enWrite = TRUE;
  for(i=0;i<FRAME_LENGTH;i++)
  {
    gComm.rx.buffer[i] = 0;
    gComm.tx.buffer[i] = 0;
  }
}

void loop()
{
  Bool ret = FALSE;

  timeNow = millis();
  switch(fsmMain)
  {
    case E_FSMMAIN_CONN_WAIT:
      if (SerialUSB)
      {
        config.ledDelay = 500;
        fsmMain = E_FSMMAIN_CONN_ESTAB;
        //Build Welcome message
      }
      break;
    case E_FSMMAIN_CONN_ESTAB:
      if (SerialUSB)
      {
        UpdateRx(); //$RASPUINO$GET/SET$something$CRC
        
        if(gComm.rx.enRead != FALSE)
        {
          ret = BuildEchoTx(gComm.rx.buffer);
        }
        
        UpdateTx();  //$ARDBERRY$STATE$something$CRC
      }
      else
      {
        fsmMain = E_FSMMAIN_CONN_CLOSED;
      }
      break;
    case E_FSMMAIN_CONN_CLOSED:
      config.ledDelay = D_LED_TIMEOUT;
      fsmMain = E_FSMMAIN_CONN_WAIT;
      break;
    default:
      break;
  }
  if ( (ledStateChTime > timeNow + config.ledDelay) ||
       (ledStateChTime + config.ledDelay < timeNow) )
  {
    LedToggleVitality();
  }
}

static void LedToggleVitality()
{
  if (ledStatePrev != FALSE)
  {
    ledStatePrev = FALSE;
    digitalWrite(config.ledPin, LOW);
  }
  else
  {
    ledStatePrev = TRUE;
    digitalWrite(config.ledPin, HIGH);
  }
  delay(config.ledDelay);
}

static UInt16 DoRx(UByte *pBuff)
{
  UInt16 size = 0;
  while ( (SerialUSB.available()>0) &&
          (size < D_SERIAL_MAX_LENGTH) )
  {
    *(pBuff+size) = SerialUSB.read();
    size++;
  }
  return size;
}

// ==================================
// |  NAME   |  CONTAINS   | LENGTH  |
// |=========|=============|=========|
// |  SYNC   |    <$>      |   3     |
// |---------|-------------|---------|
// |  HEADER |   codeA     |   1     |
// |         | body_length |   2     |
// |---------|-------------|---------|
// |  BODY   |   data      | 0-n     |
// |---------|-------------|---------|
// |  FOOTER |    CRC      |   2     |
// |---------|-------------|---------|
//Nota: conviene trasmette raw byte e non ascii soprattutto per il campo len
static void UpdateRx()
{
  UInt16 i    = 0;
  UInt16 bodyLen  = 0;
  UInt16 size = 0;
  UByte  buffer[FRAME_LENGTH];
  if(SerialUSB.available())
  {
    if(gComm.rx.enWrite!=FALSE)
    {
      size = DoRx(buffer);
    } 
    for(i=0;i<size;i++)
    {
      switch (gComm.rx.fsm)
      {
        case E_FSMRX_WSYNC1:
          gComm.rx.nrBytes = 0;
          if (buffer[i] == HVAL_SYNC1)
          {
            gComm.rx.fsm = E_FSMRX_WSYNC2;
            gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          }
          break;
        case E_FSMRX_WSYNC2:
          if (buffer[i] == HVAL_SYNC2)
          {
            gComm.rx.fsm = E_FSMRX_WSYNC3;
            gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          }
          else
          {
            gComm.rx.fsm = E_FSMRX_WSYNC1;
            gComm.rx.nrBytes = 0;
          }
          break;
        case E_FSMRX_WSYNC3:
          if (buffer[i] == HVAL_SYNC3)
          {
            gComm.rx.fsm = E_FSMRX_HEADER;
            gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          }
          else
          {
            gComm.rx.fsm = E_FSMRX_WSYNC1;
            gComm.rx.nrBytes = 0;
          }
          break;
        case E_FSMRX_HEADER:
          gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          if (gComm.rx.nrBytes >= HLENGTH)
          {
            bodyLen = gComm.rx.buffer[HPOS_LENMSB];
            bodyLen = (bodyLen << 8) + (UInt16) gComm.rx.buffer[HPOS_LENLSB];
            if (bodyLen!=0)
            {
              gComm.rx.fsm = E_FSMRX_BODY;
            }
            else
            {
              gComm.rx.fsm = E_FSMRX_FOOTER;
            }
          }
          break;
        case E_FSMRX_BODY:
          bodyLen = gComm.rx.buffer[HPOS_LENMSB];
          bodyLen = (bodyLen << 8) + (UInt16) gComm.rx.buffer[HPOS_LENLSB];
          gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          if (gComm.rx.nrBytes >= (HLENGTH+bodyLen))
          {
            gComm.rx.fsm = E_FSMRX_FOOTER;
          }
          break;
        case E_FSMRX_FOOTER:
          bodyLen = gComm.rx.buffer[HPOS_LENMSB];
          bodyLen = (bodyLen << 8) + (UInt16) gComm.rx.buffer[HPOS_LENLSB];
          gComm.rx.buffer[gComm.rx.nrBytes++] = buffer[i];
          if (gComm.rx.nrBytes > (HLENGTH+bodyLen+FLENGTH))  // FIXME > or >= ??
          {
            gComm.rx.fsm     = E_FSMRX_COMPLETE;
            gComm.rx.enRead  = TRUE;
            gComm.rx.enWrite = FALSE;
            // CHECK CRC: if bad go to E_FSMRX_BADCRC instead of E_FSMRX_COMPLETE !
          }
          break;
        case E_FSMRX_COMPLETE:
          //Do Nothing: wait for consumer to reset gComm.rx.fsms
          break;
        case E_FSMRX_BADCRC:
          //Do Nothing: wait for consumer to apply reaction
          break;
        default:
          break;
      }
    }
  }
}

static UInt16 DoTx(UByte *pBuff,UInt16 *size)
{
  UInt16 offset = 0;
  while( (SerialUSB.available()) &&
          (offset<*size) )
  {
    pBuff = pBuff+offset;
    SerialUSB.write(*pBuff);
    offset++;
  }
  //*size = 0; //do it in caller
  return offset; //so as caller can check offset==*size
}

static void UpdateTx()
{
  //implementation: polling check for data ready to tx
  //                then call DoTx
}

static Bool BuildEchoTx(UByte *pBuff)
{
  Bool   ret = FALSE;
  UInt16 len = 0;
  UInt16   i = 0;
  if(gComm.tx.enWrite != FALSE)
  {
    len = *(pBuff+HPOS_LENMSB);
    len = (len << 8) + (UInt16) *(pBuff+HPOS_LENLSB);
    for(i=0;i<HLENGTH+len+FLENGTH;i++)
    {
      gComm.tx.buffer[i] = *(pBuff+i);
    }
    gComm.tx.nrBytes = i;
    gComm.tx.length  = len;
    gComm.tx.enRead  = TRUE;
    gComm.tx.enWrite = FALSE;
    ret              = TRUE;
  }
  return ret;
}
